using System;
using System.Reflection;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

// #if X86
using EtherCAT_DLL;
// #else
// using EtherCAT_DLL_x64;
// #endif
using EtherCAT_DLL_Err;


namespace TG
{

    public enum ADCChannel : ushort
    {
        CH0 = 0,
        CH1 = 1,
        CH2 = 2,
        CH3 = 3
    }

    public enum ADCRangeMode : ushort
    {
        Positive_Negative_5V = 0,
        Positive_Negative_10V = 1
    }

    public enum ConvstFreqMode : ushort
    {
        Hz_200000 = 0,
        Hz_100000 = 1,
        Hz_50000 = 2,
        Hz_25000 = 3,
        Hz_12500 = 4,
        Hz_6250 = 5,
        Hz_3125 = 6
    }

    //电机序列下标
    public enum MotorIndex : int
    {
        BaseXAxis = 0, //底座X轴
        BaseYAxis = 1, //底座Y轴
        ChairXAxis = 3, //座椅X轴
        ChairYAxis = 2, //座椅Y轴
    }

    public class EtherCAT : MonoBehaviour
    {
        static protected EtherCAT _instance;

        static public EtherCAT Instance
        {
            get { return _instance; }
        }

        private ushort MasterCardNo = 0;

        private ushort ADNodeID = 3; //AD模块节点 配置

        private List<ushort> slaveNodeList = new List<ushort>();

        private ushort existCardCount = 0;
        private List<ushort> cardNoList = new List<ushort>();
        private bool foundADSlave = false;
        private bool initialized = false;

        private Dictionary<ushort, string> errorCodes = new Dictionary<ushort, string>();


        private BaseGameConfig gameConfig;

        public int standReductRatioX = 480;
        public int standReductRatioY = 100;
        public int seatReductRatioX = 300;
        public int seatReductRatioY = 100;

        void Awake()
        {
            if (_instance != null)
            {
                Destroy(gameObject);
                return;
            }

            _instance = this;

            DontDestroyOnLoad(gameObject);

        }



        public void Initialize(BaseGameConfig config)
        {

            gameConfig = config;
            ushort value = 0;
            if (gameConfig.posture == Posture.Stand)
            {
                if (!ushort.TryParse(gameConfig.baseADNodeID, out value))
                {

                    //UIManager.Instance.ErrorBox("底座AD节点ID参数配置错误");
                    return;
                }
            }
            else if (gameConfig.posture == Posture.Sit)
            {
                if (!ushort.TryParse(gameConfig.chairADNodeID, out value))
                {

                    //UIManager.Instance.ErrorBox("座椅AD节点ID参数配置错误");
                    return;
                }
            }


            ADNodeID = value;


            PrepareErrorCodes();


            StartCoroutine(InitialCard());


        }

        private IEnumerator InitialCard()
        {

            if (!initialized)
            {
                try
                {

                    uRet = CEtherCAT_DLL.CS_ECAT_Master_Open(ref existCardCount);
                    if (existCardCount == 0)
                    {
                        Debug.LogWarning("No EtherCat can be found!");
                        //UIManager.Instance.ErrorBox("没有发现设备EtherCat MasterCard");
                        yield break;
                    }

                    ushort uCount = 0, uCardNo = 0;
                    cardNoList.Clear();
                    for (uCount = 0; uCount < existCardCount; uCount++)
                    {
                        uRet = CEtherCAT_DLL.CS_ECAT_Master_Get_CardSeq(uCount, ref uCardNo);
                        if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
                        {
                            Debug.Log("CS_ECAT_Master_Get_CardSeq, " + GetEtherCATErrorCode(uRet));
                        }
                        else
                        {
                            cardNoList.Add(uCardNo);
                        }
                    }

                    if (cardNoList.Count <= 0)
                    {
                        //  Debug.LogWarning("No EtherCat can be found" );
                        //UIManager.Instance.ErrorBox("没有发现设备EtherCat MasterCard");
                        yield break;
                    }

                    MasterCardNo = cardNoList[0];

                    uRet = CEtherCAT_DLL.CS_ECAT_Master_Initial(MasterCardNo);
                    if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
                    {
                        Debug.Log("CS_ECAT_Master_Initial, " + GetEtherCATErrorCode(uRet));
                        yield break;
                    }
                }
                catch (Exception ex)
                {
                    Debug.LogWarning(ex.Message);
                    yield break;
                }


                uInitialDone = 100;
                InvokeRepeating("CheckInitialDone", 0f, 0.2f);
                //检查初始化状态，等待初始化成功uInitialDone == 0,成功后才能获取Slave信息
                yield return new WaitUntil(() => uInitialDone == 0);
                CancelInvoke("CheckInitialDone");

                FindSlave();

                initialized = true;
            }


            InitTrainMode();



        }

        private void FindSlave()
        {
            try
            {
                ushort uNID = 0, uSlaveNum = 0, uReMapNodeID = 0;
                uint uVendorID = 0, uProductCode = 0, uRevisionNo = 0, uSlaveDCTime = 0;
                foundADSlave = false;
                slaveNodeList.Clear();

                uRet = CEtherCAT_DLL.CS_ECAT_Master_Get_SlaveNum(MasterCardNo, ref uSlaveNum);
                if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
                {
                    Debug.Log("CS_ECAT_Master_Get_SlaveNum, " + GetEtherCATErrorCode(uRet));
                }
                else
                {
                    if (uSlaveNum == 0)
                    {
                        Debug.Log("Card NO: " + MasterCardNo.ToString() + " No slave found!");
                    }
                    else
                    {
                        for (uNID = 0; uNID < uSlaveNum; uNID++)
                        {
                            uRet = CEtherCAT_DLL.CS_ECAT_Master_Get_Slave_Info(MasterCardNo, uNID, ref uReMapNodeID,
                                ref uVendorID, ref uProductCode, ref uRevisionNo, ref uSlaveDCTime);
                            if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
                            {
                                Debug.Log("CS_ECAT_Master_Get_Slave_Info, " + GetEtherCATErrorCode(uRet));
                                continue;
                            }

                            Debug.Log(string.Format(
                                "NID : {0}, NodeID : {1}, VendorID : {2}, ProductCode : {3}, RevisionNo : {4}", uNID,
                                uReMapNodeID, uVendorID, uProductCode, uRevisionNo));

                            slaveNodeList.Add(uNID);
                        }
                    }
                }


            }
            catch (Exception ex)
            {
                Debug.LogWarning(ex.Message);
            }

            Debug.Log("Slave Node Count: " + slaveNodeList.Count);


        }

        private void InitTrainMode()
        {

            CloseTrainMode();


            if (gameConfig.passive.enable)
            {
                InitADModule();
                InvokeRepeating("GetADInputValue", 0f, 0.1f);

            }
            else if (gameConfig.initiative.enable)
            {
                //主动模式
                if (slaveNodeList.Count < NumberOfEnumValues(typeof(MotorIndex)))
                {
                    //UIManager.Instance.ErrorBox("没有找到底座或者座椅运动设备!");
                    return;
                }

                if (gameConfig.posture == Posture.Stand)
                {
                    InvokeRepeating("GetBaseAxisInputValue", 0f, 0.1f);
                }
                else if (gameConfig.posture == Posture.Sit)
                {
                    InvokeRepeating("GetChairAxisInputValue", 0f, 0.1f);
                }

                if (gameConfig.rolloverModel != 2)
                {
                    InvokeRepeating("GetADAxisInputValue", 0f, 0.1f);
                }
            }
            else
            {
                //静态模式
                InitADModule();
                InvokeRepeating("GetADInputValue", 0f, 0.1f);
            }



        }

        public void CloseTrainMode()
        {
            //主动模式
            CancelInvoke("GetBaseAxisInputValue");
            CancelInvoke("GetChairAxisInputValue");
            //静态模式 被动模式
            CancelInvoke("GetADInputValue");
        }

        private void InitADModule()
        {
            SetInputEnable(ADCChannel.CH0, true);
            SetInputEnable(ADCChannel.CH1, true);
            SetInputEnable(ADCChannel.CH2, true);
            SetInputEnable(ADCChannel.CH3, true);

            SetInputRangeMode(ADCChannel.CH0, ADCRangeMode.Positive_Negative_5V);
            SetInputRangeMode(ADCChannel.CH1, ADCRangeMode.Positive_Negative_5V);
            SetInputRangeMode(ADCChannel.CH2, ADCRangeMode.Positive_Negative_5V);
            SetInputRangeMode(ADCChannel.CH3, ADCRangeMode.Positive_Negative_5V);
        }



        double[] ADValue = new double[4];

        private void GetADInputValue()
        {
            for (int ch = 0; ch < 4; ch++)
            {
                ADValue[ch] = GetInputValue((ADCChannel)ch);
                ADValue[ch] = (ADValue[ch] / 5 * 200);
            }

            ADValue[0] -= MainGame.Instance.curGameConfig.sensor1;
            ADValue[1] -= MainGame.Instance.curGameConfig.sensor2;
            ADValue[2] -= MainGame.Instance.curGameConfig.sensor3;
            ADValue[3] -= MainGame.Instance.curGameConfig.sensor4;
            BalanceInputManager.Instance.SetValue(ref ADValue);
        }

        ushort uConnectStatus = 0, uRet = 0, uAbnormal_Flag = 0, uWorking_Slave_Cnt = 0, uInitialDone = 0;


        private void CheckInitialDone()
        {
            try
            {
                uRet = CEtherCAT_DLL.CS_ECAT_Master_Check_Initial_Done(MasterCardNo, ref uInitialDone);
                if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
                {
                }
            }
            catch (Exception ex)
            {
                Debug.LogWarning(ex.Message);
            }
        }

        private void CheckWorkingStatus()
        {
            try
            {
                uRet = CEtherCAT_DLL.CS_ECAT_Master_Check_Working_Counter(MasterCardNo, ref uAbnormal_Flag,
                    ref uWorking_Slave_Cnt);
                if (uRet == 0)
                {
                    if (uAbnormal_Flag == 0)
                    {
                        //Normal
                        // Debug.Log("CS_ECAT_Master_Check_Working_Counter Status Normal");
                    }
                    else
                    {
                        //Abnormal
                        Debug.Log("CS_ECAT_Master_Check_Working_Counter Status Abnormal");
                    }

                }
            }
            catch (Exception ex)
            {
                Debug.LogWarning(ex.Message);
            }
        }

        private int NumberOfEnumValues(System.Type type)
        {
            return type.GetFields(BindingFlags.Public | BindingFlags.Static).Length;
        }

        int[] axisValue = new int[2];

        private void GetBaseAxisInputValue()
        {

            axisValue[0] = GetMotionPosition(slaveNodeList[(int)MotorIndex.BaseXAxis]);
            axisValue[1] = GetMotionPosition(slaveNodeList[(int)MotorIndex.BaseYAxis]);
            BalanceInputManager.Instance.SetAxisValue(ref axisValue);

        }

        private void GetChairAxisInputValue()
        {
            axisValue[0] = GetMotionPosition(slaveNodeList[(int)MotorIndex.ChairXAxis]);
            axisValue[1] = GetMotionPosition(slaveNodeList[(int)MotorIndex.ChairYAxis]);
            BalanceInputManager.Instance.SetAxisValue(ref axisValue);
        }

        private void GetADAxisInputValue()
        {

            for (int ch = 0; ch < 4; ch++)
            {
                ADValue[ch] = GetInputValue((ADCChannel)ch);
                ADValue[ch] = (ADValue[ch] / 5 * 200);
            }

            ADValue[0] -= MainGame.Instance.curGameConfig.sensor1;
            ADValue[1] -= MainGame.Instance.curGameConfig.sensor2;
            ADValue[2] -= MainGame.Instance.curGameConfig.sensor3;
            ADValue[3] -= MainGame.Instance.curGameConfig.sensor4;

            axisValue[0] = GetMotionPosition(slaveNodeList[(int)MotorIndex.BaseXAxis]);
            axisValue[1] = GetMotionPosition(slaveNodeList[(int)MotorIndex.BaseYAxis]);

            BalanceInputManager.Instance.SetAxisValueAndValue(ref ADValue, ref axisValue);
        }

        public void SetInputRangeMode(ADCChannel channelNo, ADCRangeMode rangeMode)
        {
            try
            {
                uRet = CEtherCAT_DLL.CS_ECAT_Slave_R1_EC8124_Set_Input_RangeMode(MasterCardNo, ADNodeID,
                    (ushort)channelNo, (ushort)rangeMode);
                if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
                {
                    Debug.Log("CS_ECAT_Slave_R1_EC8124_Set_Input_RangeMode, " + GetEtherCATErrorCode(uRet));
                }
            }
            catch (Exception ex)
            {
                Debug.LogWarning(ex.Message);
            }

        }

        public ushort GetInputRangeMode(ADCChannel channelNo)
        {
            ushort rangeMode = 0;
            try
            {
                uRet = CEtherCAT_DLL.CS_ECAT_Slave_R1_EC8124_Get_Input_RangeMode(MasterCardNo, ADNodeID,
                    (ushort)channelNo, ref rangeMode);
                if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
                {
                    Debug.Log("CS_ECAT_Slave_R1_EC8124_Get_Input_RangeMode, " + GetEtherCATErrorCode(uRet));
                }
            }
            catch (Exception ex)
            {
                Debug.LogWarning(ex.Message);
            }

            return rangeMode;
        }

        public void SetInputConvstFreqMode(ADCChannel channelNo, ConvstFreqMode rangeMode)
        {
            try
            {
                uRet = CEtherCAT_DLL.CS_ECAT_Slave_ESC8124_Set_Input_ConvstFreq_Mode(MasterCardNo, ADNodeID,
                    (ushort)channelNo, (ushort)rangeMode);
                if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
                {
                    Debug.Log("CS_ECAT_Slave_ESC8124_Set_Input_ConvstFreq_Mode, " + GetEtherCATErrorCode(uRet));
                }
            }
            catch (Exception ex)
            {
                Debug.LogWarning(ex.Message);
            }
        }

        public void SetInputEnable(ADCChannel channelNo, bool enable)
        {
            try
            {
                uRet = CEtherCAT_DLL.CS_ECAT_Slave_R1_EC8124_Set_Input_Enable(MasterCardNo, ADNodeID, (ushort)channelNo,
                    (ushort)(enable ? 1 : 0));
                if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
                {
                    Debug.Log("CS_ECAT_Slave_R1_EC8124_Set_Input_Enable, " + GetEtherCATErrorCode(uRet));
                }
            }
            catch (Exception ex)
            {
                Debug.LogWarning(ex.Message);
            }
        }

        public void SetInpuAverageMode(ADCChannel channelNo, ushort averageTimes)
        {
            try
            {
                uRet = CEtherCAT_DLL.CS_ECAT_Slave_ESC8124_Set_Input_AverageMode(MasterCardNo, ADNodeID,
                    (ushort)channelNo, averageTimes);
                if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
                {
                    Debug.Log("CS_ECAT_Slave_ESC8124_Set_Input_AverageMode, " + GetEtherCATErrorCode(uRet));
                }
            }
            catch (Exception ex)
            {
                Debug.LogWarning(ex.Message);
            }
        }

        public void SetMotionEnable(ushort nodeID, bool enable)
        {
            try
            {
                uRet = CEtherCAT_DLL.CS_ECAT_Slave_Motion_Set_Svon(MasterCardNo, nodeID, 0, (ushort)(enable ? 1 : 0));
                if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
                {
                    Debug.Log("CS_ECAT_Slave_Motion_Set_Svon, " + GetEtherCATErrorCode(uRet));
                }
            }
            catch (Exception ex)
            {
                Debug.LogWarning(ex.Message);
            }
        }

        public int GetMotionPosition(ushort nodeID)
        {
            int nPos = 0;
            try
            {
                uRet = CEtherCAT_DLL.CS_ECAT_Slave_Motion_Get_Position(MasterCardNo, nodeID, 0, ref nPos);
                if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
                {
                    Debug.Log("CS_ECAT_Slave_Motion_Set_Svon, " + GetEtherCATErrorCode(uRet));
                }
            }
            catch (Exception ex)
            {
                Debug.LogWarning(ex.Message);
            }

            return nPos;
        }

        public double GetInputValue(ADCChannel channelNo)
        {
            double value = 0;
            try
            {

                uRet = CEtherCAT_DLL.CS_ECAT_Slave_R1_EC8124_Get_Input_Value(MasterCardNo, ADNodeID, (ushort)channelNo,
                    0, ref value);
                if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
                {
                    Debug.Log("CS_ECAT_Slave_R1_EC8124_Get_Input_Value, " + GetEtherCATErrorCode(uRet));
                }

            }
            catch (Exception ex)
            {
                Debug.LogWarning(ex.Message);
            }

            return value;
        }

        public void StartMoveMotor(int angle)
        {
            if (gameConfig != null && gameConfig.passive.enable)
            {
                if (gameConfig.posture == Posture.Stand)
                {
                    StartMoveBaseYAxis(angle);
                }
                else if (gameConfig.posture == Posture.Sit)
                {
                    StartMoveChairYAxis(angle);
                }
            }
        }

        //底座横轴运动
        public void StartMoveBaseXAxis(int angle)
        {
            int pluse = (int)(angle / 360f * standReductRatioX * 100000);
            AxisMove((int)MotorIndex.BaseXAxis, pluse);
        }

        //底座纵轴运动
        public void StartMoveBaseYAxis(int angle)
        {
            int pluse = (int)(angle / 360f * standReductRatioY * 100000);
            AxisMove((int)MotorIndex.BaseYAxis, pluse);
        }

        //座椅横轴运动
        public void StartMoveChairXAxis(int angle)
        {
            int pluse = (int)(angle / 360f * seatReductRatioX * 100000);
            AxisMove((int)MotorIndex.ChairXAxis, pluse);
        }

        //座椅纵轴运动
        public void StartMoveChairYAxis(int angle)
        {
            int pluse = (int)(angle / 360f * seatReductRatioY * 100000);
            AxisMove((int)MotorIndex.ChairYAxis, pluse);
        }

        public void AxisMove(int index, int pluse)
        {
            if (slaveNodeList.Count < index + 1)
            {
                return;
            }

            Debug.Log(
                "StartAxisMove MasterCardNo" + MasterCardNo + " NodeNo" + slaveNodeList[index] + " pluse " + pluse);
            uRet = CEtherCAT_DLL.CS_ECAT_Slave_CSP_Start_Move(MasterCardNo, slaveNodeList[index], 0, pluse, 100000,
                300000, 0, 0.1f, 0.1f, 0, 1);
            if (uRet != CEtherCAT_DLL_Err.ERR_ECAT_NO_ERROR)
            {
                Debug.Log("CS_ECAT_Slave_CSP_Start_Move, " + GetEtherCATErrorCode(uRet));
            }
        }

        void OnDestroy()
        {
        }

        #region ErrorCode

        private string GetEtherCATErrorCode(ushort errorCode)
        {
            string errorDescription = string.Format(" ErrorCode : {0}", IntToHexString(errorCode));
            if (errorCodes.ContainsKey(errorCode))
            {
                errorDescription = string.Format("{0}, {1}", errorDescription, errorCodes[errorCode]);
            }

            return errorDescription;
        }

        private void PrepareErrorCodes()
        {
            if (errorCodes.Count == 0)
            {
                System.Type errorCodeType = typeof(CEtherCAT_DLL_Err);

                FieldInfo[] infoArray = errorCodeType.GetFields();

                foreach (FieldInfo info in infoArray)
                {
                    if (info.FieldType == typeof(ushort))
                    {
                        errorCodes.Add((ushort)info.GetValue(info.Name), info.Name);
                    }
                }
            }
        }


        private string IntToHexString(ushort value)
        {
            return "0x" + string.Format("{0:X}", value);
        }

        #endregion
    }
}
